# 导入 __future__ 文件的 division 是为了精确除法。
from __future__ import division

# 导入Adafruit_PCA9685模块
import Adafruit_PCA9685
from topic_msg.msg import CatSteer


class Steers:
    def __init__(self):
        # 使用默认地址（0x40）初始化PCA9685。
        # 把Adafruit_PCA9685.PCA9685()引用地址赋给PWM标签
        # 或者指定不同的地址和/或总线：
        self._pwm = Adafruit_PCA9685.PCA9685(busnum=3)
        # 将频率设置为50赫兹，适合伺服系统。
        self._pwm.set_pwm_freq(50)

    def _set_servo_angle(self, channel, angle):  # 输入角度转换成12^精度的数值
        "选择需要移动的伺服电机的通道与角度"
        if angle >= 0 and angle <= 180:
            # + 0.5是进行四舍五入运算。
            date = int(4096 * ((angle * 11) + 500) / 20000 + 0.5)
            self._pwm.set_pwm(channel, 0, date)
        else:
            print(f"input invalid angle")

    def set_group_servo(self, group_cmd):
        if group_cmd.number != len(group_cmd.steer) or group_cmd.number != len(
            group_cmd.angle
        ):
            return

        for i in range(group_cmd.number):
            self._set_servo_angle(group_cmd.steer[i], group_cmd.angle[i])
